import numpy as np
#from matplotlib import pyplot as plt
import shape
import math
# import B_Spline as bsl

class Scene():

    def __init__(self,border,dense,barriers,shape='t', start = [0,0]):
        self.border = np.array(border)
        self.dense = dense
        self.joint = np.array([start])
        self.barriers = np.array(barriers)
        self.cmap = np.array([])
        self.shape = shape
        self.visibility_dict = {}
        self.curvature_dict = {}

        gap = round(dense/2)
        step_x = border[0] + gap
        step_y = border[1] + gap
        toggle = 0

        while(step_y < border[3] - gap):
            while(step_x < border[2] - gap):
                d_min = 9999
                for barrier in self.barriers:
                    d = barrier.calc_distance(step_x,step_y)
                    if (d < d_min):
                        d_min = d
                if (d_min < self.dense * 0.75):
                    pass
                    #self.cmap = np.append(self.cmap,'r')
                else:
                    self.cmap = np.append(self.cmap,'b')
                    self.joint = np.append(self.joint,np.array([[step_x,step_y]]),axis=0)
                step_x += self.dense

            if (self.shape == 's'): # 四方排布
                step_x = border[0] + gap
                step_y += self.dense

            elif (self.shape == 't'): # 三角排布
                toggle = 1 - toggle
                step_x = border[0] + gap * (1 + toggle)
                step_y += self.dense * 0.8660254
            else:
                break
        # self.joint = self.joint[1:,:]

    def calc_tilt(self,p):
        sign_fx = 0;
        x = p[0]
        y = p[1]

        if(x > 0):
            sign_fx = 1
        elif (x < 0):
            sign_fx = -1
        fx = (1 - sign_fx) / 2
        fy = 1 if (y>0) else -1
        if (x == 0):
            deg = fx * fy * math.pi
        else:
            deg = math.atan(y/x) + fx * fy * math.pi
        return deg * 180 / math.pi

    def deg_fix(self,deg):
        while(deg>180):
            deg -= 360
        while(deg<-180):
            deg += 360
        return deg

    def norm_abs_deg_fix(self,deg):
        return abs(self.deg_fix(deg))/180

    def calc_dict(self):
        j_len = len(self.joint)
        for i in range(j_len):
            for j in range(i+1,j_len): #计算距离
                dx = self.joint[j][0]-self.joint[i][0]
                dy = self.joint[j][1]-self.joint[i][1]
                d = math.sqrt(dx*dx + dy*dy)
                cross = 0
                vis_value = 0.0
                # 判断是否越过障碍
                for barrier in self.barriers:
                    cross = cross or barrier.if_near_ring(self.joint[i],self.joint[j])
                if (cross == 0):
                    if d == 0:
                        vis_value = self.dense
                    else:
                        vis_value = round(self.dense/d,7)
                        
                self.visibility_dict['{}-{}'.format(i,j)] = vis_value

                for k in range(j+1,j_len): #计算夹角
                    # c1 = cross
                    # c2 = 0
                    # c3 = 0
                    # for barrier in self.barriers:
                    #     c2 = c2 or barrier.if_cross(self.joint[j],self.joint[k])
                    #     c3 = c3 or barrier.if_cross(self.joint[k],self.joint[i])
                    t1 = self.calc_tilt(self.joint[j]-self.joint[i])
                    t2 = self.calc_tilt(self.joint[k]-self.joint[j])
                    t3 = self.calc_tilt(self.joint[i]-self.joint[k])
                    a1 = self.norm_abs_deg_fix(t1 - self.deg_fix(t3 + 180))#*(1-(c1 or c3))
                    a2 = self.norm_abs_deg_fix(t2 - self.deg_fix(t1 + 180))#*(1-(c1 or c2))
                    a3 = self.norm_abs_deg_fix(t3 - self.deg_fix(t2 + 180))#*(1-(c2 or c3))
                    #print(t1,t2,t3)
                    self.curvature_dict['{}-{}-{}'.format(i,j,k)] = a1
                    self.curvature_dict['{}-{}-{}'.format(j,i,k)] = a2
                    self.curvature_dict['{}-{}-{}'.format(k,i,j)] = a3

    def sql_curv(self,pre,end):
        curv_list = []
        for i in range(len(self.joint)):
            if (i==pre or i==end):
                curv_list.append(0.0)
            elif (pre == end):
                curv_list.append(1.0)
            elif (i<pre):
                curv_list.append(self.curvature_dict['{}-{}-{}'.format(end,i,pre)])
            else:
                curv_list.append(self.curvature_dict['{}-{}-{}'.format(end,pre,i)])
        return np.array(curv_list)

    def sql_vis(self,end):
        vis_list = []
        for i in range(len(self.joint)):
            if (i==end):
                vis_list.append(0.0)
            elif (i<end):
                vis_list.append(self.visibility_dict['{}-{}'.format(i,end)])
            else:
                vis_list.append(self.visibility_dict['{}-{}'.format(end,i)])
        return np.array(vis_list)


                

# border = [-25,-25,500,325]

# pillar1 = shape.Pillar([100,140],45,100)
# pillar2 = shape.Pillar([300,70],-72,100)

# sc = Scene(border,50,[pillar1,pillar2],'t')
# sc.calc_dict()
# joints = sc.joint
# px = joints[:,:1].flatten()
# py = joints[:,1:].flatten()

# vis_list = sc.sql_vis(10)
# cur_list = sc.sql_curv(9,10)
# print(vis_list)
# print(cur_list)


# plt.title('Scene')
# plt.scatter(px,py,s=3,c=sc.cmap)
# plt.show()

